
(* 
**********************************************************

		 COSY_PAK package chap7.m		

**********************************************************
*)

If [ TrueQ[ $VersionNumber >= 2.0 ],
     Off[ General::spell ];
     Off[ General::spell1 ] ]
     
(*  B E G I N     P A C K A G E  *)

BeginPackage["COSYPAK`chap7`",
		"LinearAlgebra`MatrixManipulation`", 
		"SignalProcessing`Analog`LaPlace`",
		"SignalProcessing`Analog`InvLaPlace`"]

matrixpower::usage = 
	"matrixpower[A,n] returns the result of A^n."
(*	Mathematica's multiplication of A^2 != A.A  *)
(*	made function to fix this  *)

Controllable::usage = 
		"Controllable[A, B] returns a logic value 
		representing the complete state 
		controllability of system A, B."

PolePlaceGain::usage =
		"PolePlaceGain[A, B, poles] returns gain matirix K
		 to place poles of system A - BK at 
		 locations specified by poles."

ObsPolePlace::usage =
		"ObsPolePlace[A, C, newpoles] determines 
		state observer gain matrix using 
		Ackermann's formula."
		
rank::usage =
		"rank[A] returns rank of matrix A."

Observable::usage =
		"Observable[A, C] returns gain matrix to 
		place observer poles."

sspace::usage =
		"sspace[a, b] returns the state-space form of 
		the ordinary differential equation such as
	 y''' + a2 y'' + a3 y' + a4 y = b1 u' + b2 u 
	 with the coefficients of the y variables 
	 given by the list a and the coefficients 
	 of the u variables given by the list b.  
	 The list a must start with a 1 and the list 
	 b should be padded with leading zeroes 
	 if it is not the same length as a. The A,
	 B, C, and D matrices of the equations
	 x'= Ax + Bu
	 y = Cy + Du
	 as the global values AOUT, BOUT, COUT,
	 and DOUT."

ExpAt::usage=
		"ExpAt[a] returns exponential matrix of A."

SysResponse::usage=
		"SysResponse[A, B, C, x0, input, s, {t, tmin, tmax}, gopts]: \
		Graphs output of system y and returns time domain \
		solutions for state x and output y. Returns graphics."


OutControllable::usage=
		"OutControllable[A, B, C, D] returns a logic value 
		representing the output 
		controllability of system A, B, C, D."

tpose::usage=
		"tpose[A] returns the transpose of 
		the matrix A."


Begin["`private`"]

rank[a_]:= Block[{red,size,rnk,nz,i,j},
	rnk= 0;
	size= Dimensions[a];
	red= RowReduce[a];
	Do[ nz= False;
		Do[ If [red[[i,j]]!= 0, nz= True], 
			{j,size[ [2] ]} ];
		If [ nz, rnk= rnk + 1], {i, size[ [1] ]} ];
	rnk
	];
	

matrixpower[matrix_,power_Integer?Positive]:=
	Block[ {ii,temp,size},
	size= Dimensions[matrix];
	temp= IdentityMatrix[ size[  [1]  ]  ];
	Do[ temp= temp . matrix, {ii,power} ]; 
	temp
	];
	

Controllable[a_,b_]:=Block[{i, augm, dimena, rowa, cola,
	dimenb, rowb, colb},
	
	(* Get sizes of matrices *)
	dimena= Dimensions[a];
	dimenb= Dimensions[b];
	rowa= dimena[ [1] ];		
	cola= dimena[ [2] ];
	rowb= dimenb[ [1] ];		
	colb= dimenb[ [2] ];
	
	If[rowa != cola, Print["A must be a square matrix."]];

	augm= b;
	
	(* Form augmented matrix *)
	Do[augm= AppendRows[augm,matrixpower[a,i].b],
		{i,(rowa-1)}];

	(* Check if controllability *)
	(* matrix is of full rank   *)
	rank[augm] == rowa
	];
	

PolePlaceGain[a_,b_,newpoles_]:= Block[{m1, s, dimnpoles,
	phi, i, tot, temp, j, dimena, rowa, m2},

	If[ Controllable[a,b],
	dimnpoles= Dimensions[newpoles];
	
	(*  Ackermann's formula  *)
	phi= Expand[ Product[ s - newpoles[ [i] ],
		{ i,dimnpoles[ [1] ] } ] ];
	tot= Coefficient[phi,s,0] IdentityMatrix
		[dimnpoles[ [1] ] ];
	Do[tot= tot + 
		Coefficient[phi,s,i] matrixpower[a,i],
		{i,1,dimnpoles[ [1] ]} ];
		
	If[ dimnpoles[ [1] ] == 1, m1= {1}, m1= {0};
		Do [AppendTo[m1,0], {i, 1, (dimnpoles[[1]]-2)} ];
		AppendTo[ m1,1 ]; ];
		m1= {m1};
		
	 	(*  controllability matrix *)
	 	dimena= Dimensions[ a ];
		rowa= dimena[ [1] ];
		m2= b;
		Do[m2= AppendRows[m2, matrixpower[a,i].b],
		 {i,(rowa-1)} ];

	(*  find gain matrix K  *)
	m1 . Inverse[ m2 ] . tot,
	
	(* Not Controllable *)
	Print["The system is not controllable."]]
	];
	

ObsPolePlace[a_,c_,newpoles_]:= Block[{m1, s, dimnpoles,
	phi, i, tot, temp, j, dimena, rowa, m2},
	
		(*  full state observer  *)
	
	If[Observable[a,c],
	dimnpoles= Dimensions[newpoles];
	
	(*  Ackermann's formula  *)
	phi= Expand[ Product[ s - newpoles[ [i] ],
		{ i,dimnpoles[ [1] ] } ] ];
	tot=Coefficient[phi,s,0] IdentityMatrix
		[dimnpoles[ [1] ] ];
	Do[tot= tot + 
		Coefficient[phi,s,i] matrixpower[a,i],
		{i,1,dimnpoles[ [1] ]} ];
		
	If[ dimnpoles[ [1] ] == 1, m1= {{1}}, m1= {{0}};
		Do [ m1= AppendColumns[m1, {{0}} ],
			{i, 1, (dimnpoles[[1]]-2)} ];
		m1= AppendColumns[ m1, {{1}} ]; ];
		
	 	(*  observability matrix *)
	 	dimena= Dimensions[ a ];
		rowa= dimena[ [1] ];
		m2= c;
		Do[m2= AppendColumns[m2, c . matrixpower[a,i] ],
		 {i,(rowa-1)} ];

	(*  find gain matrix K  *)
	tot . Inverse[ m2 ] . m1,
	
	(* Not Observable *)
	Print["The system is not observable."]]
	];


tpose[a_]:= Block[{i,j,size,row,col,temp},
	(* get # of rows and cols of a *)
	size= Dimensions[a];
	row= size[[1]];
	col= size[[2]];
	
	(* initialize temp matrix *)
	temp= Array[1,{col,row}];
	
	(* transpose elements *)
	Do[
		Do[temp[[j,i]]= a[[i,j]], { j, size[[2]] }],
		{ i, size[[1]] }];
	temp ];

	
Observable[a_,c_]:=Block[{i,augm,dimena,rowa,cola,dimenc
	,rowc,colc},
	
	(* Get sizes of matrices *)
	dimena=Dimensions[a];
	dimenc=Dimensions[c];
	rowa=dimena[[1]];
	cola=dimena[[2]];
	rowc=dimenc[[1]];
	colc=dimenc[[2]];
	
	If[rowa != cola, Print["A must be a square matrix."]];

	(* Begin augmented matrix *)
	augm= c;
	
	(* Form augmented matrix *)
	Do[augm= AppendColumns[augm,
		c . matrixpower[a,i] ],
		{i,(rowa-1)} ];
		
	(* Check if observability *)
	(* matrix is of full rank   *)
	rank[augm] == rowa
	];
	
	
	sspace[a_,b_]:= Block[{na,nb,beta,temp},
	(* Convert ordinary differential *)
	(* equation to state space *)

	(* find size of input lists *)
	na= Dimensions[a];
	na= na[[1]];
	
	nb= Dimensions[b];
	nb= nb[[1]];
	
	(* lists must be same size *)
	If[nb != na,Print["Coefficient lists a and b",
		" must be the same size.  Remember to pad",
		" the lists with leading zeros, if needed."] ];
		
	beta= {b[[1]]};
	
	Do[ temp= b[[i]];
		Do[ temp= temp - a[[j]] beta[[i-j+1]], {j,2,i}];
		AppendTo[ beta, temp], {i,2,nb}];
		
	AOUT= IdentityMatrix[na-1];
	Do[ Do[ If[ (j-1) == i, AOUT[[i,j]]= 1, AOUT[[i,j]]= 0],
			{i, na-2}];
		AOUT[[na-1,j]]= -a[[na+1-j]], {j,na-1} ];
		
	BOUT= {{beta[[2]]}};
	Do[ BOUT= AppendColumns[ BOUT, {{beta[[i]]}} ],
		{i, 3, na}];
	
	COUT= {{1}};
	Do[ COUT=AppendRows[COUT,{{0}}], {na-2}];
	
	DOUT= beta[[1]];
		
	Print["The A Matrix is:"];
	Print[" "]; Print[" "];
	Print[" ",TableForm[AOUT]];
	Print[" "]; Print[" "];
	Print["The B Matrix is:"];
	Print[" "]; Print[" "];
	Print[" ",TableForm[BOUT]];
	Print["The C Matrix is:"];
	Print[" "]; Print["  "];
	Print[" ",TableForm[COUT]];
	Print[" "]; Print["  "];
	Print["The D Matrix is:"];
	Print[" "]; Print["  "];
	Print[" ",TableForm[DOUT]];
];


OutControllable[a_,b_,c_,d_]:=Block[{i, augm, dimena, dimenc, 
	rowa, cola, dimenb, rowb, colb, rowc},
	
	(* Get sizes of matrices *)
	dimena= Dimensions[a];
	dimenb= Dimensions[b];
	dimenc= Dimensions[c];
	rowa= dimena[ [1] ];		
	cola= dimena[ [2] ];
	rowb= dimenb[ [1] ];		
	colb= dimenb[ [2] ];
	rowc= dimenc[ [1] ];		

	
	If[rowa != cola, Print["A must be a square matrix."]];

	augm= c.b;
	
	(* Form augmented matrix *)
	Do[augm= AppendRows[augm,c.matrixpower[a,i].b],
		{i,(rowa-1)}];
	
	AppendRows[augm, d];

	(* Check if controllability *)
	(* matrix is of full rank   *)
	rank[augm] == rowc
	];


SysResponse[a_, b_, c_, x0_, input_,s_,{t_,tmin_,tmax_}, opts___]:=
	Block[{tau, intexp, x, y, dimena,dimenb,rowa,cola,rowb,colb,
		dimenc,colc,intu,II,exps},
		(* Get sizes of matrices *)
		dimena= Dimensions[a];
		dimenb= Dimensions[b];
		dimenc= Dimensions[c];
		rowa= dimena[[1]];		
		cola= dimena[[2]];	
		colb= dimenb[[1]];
		colc= dimenc[[1]];
	
		If[rowa != cola, Print["expAt must be a square matrix."];
			Return[{}]];
		If[rowa != colb, Print["expAt and B must have an equal \
		number of columns"];Return[{}]];
		If[cola != colc, Print["expAt and C must have an equal \
		number of rows"];Return[{}]];

		II=IdentityMatrix[rowa]; 	
        	exps =Together[Inverse[s II -a]];
   		inputs = LaPlace[input,t,s][[1]];
		
		initpart = exps.x0;
		Do[initpart[[i]]= Simplify[ComplexExpand[
			InvLaPlace[initpart[[i]],s,t, Apart->All]]],{ i, cola }];
			
		forcepart = exps.b inputs;
		Do[forcepart[[i]]= Simplify[ComplexExpand[
			InvLaPlace[forcepart[[i]],s,t, Apart->All]]],{ i, cola }];
		
		
		(* integrate and add initial *)
		(* conditions to find x and y *)
		x= initpart + forcepart;
		y= c.x ;
		
		(* output graph and solutions *)		
		Plot[y,{t,tmin,tmax}, 
		FrameLabel->{"Time","Y(t)","Time Response of System "," "},
		Frame->True,PlotRange -> All,GridLines->Automatic, 
        	FrameTicks -> {Automatic, Automatic,Automatic, Automatic},
		opts];
		Return[{x,y}]
	];
				

		
ExpAt[a_]:= 
Block[{i,j,out,xform, II, nn},
	nn = Dimensions[a][[1]];
	II= IdentityMatrix[nn];
	xform= Inverse[s II - a];
	Do[
	  Do[ 
	  out[[i,j]]= Simplify[ComplexExpand[InvLaPlace[ xform[[i,j]],s]]],
	 { j, nn}],
	 { i, nn}];
	
	Return[out]
	];

	
	End[]
	EndPackage[]


(*  E N D     P A C K A G E  *)

If [ TrueQ[ $VersionNumber >= 2.0 ],
     On[ General::spell ];
     On[ General::spell1 ] ]
